/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "PIDController.h"

#include "Utils.h"
#include <sstream>

#include <cmath>
#include <limits>

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const std::string PIDController::CONF_ALL = "*";
const std::string PIDController::CONF_GAIN = "gain";
const std::string PIDController::CONF_B = "b";
const std::string PIDController::CONF_TI = "ti";
const std::string PIDController::CONF_TD = "td";
const std::string PIDController::CONF_N = "n";
const std::string PIDController::CONF_UBOUND = "upper_bound";
const std::string PIDController::CONF_LBOUND = "lower_bound";

PIDController::PIDController(SimInterface::ISimulator &isim, double gain,
                             double ti, double td)
    : ControllerBase(isim, "PID", "PIDController"),
      m_k(gain), m_b(1.0),
      m_ti(ti), m_tiSum(0),
      m_td(td), m_N(10),
      m_upperBound(2000), m_lowerBound(-2000),
      m_Dm1(0),
      m_em1(0)
{
    reset();
}

void PIDController::reset()
{
    m_tiSum = m_em1 = 0;
}

double PIDController::simulateControllerStep(double setPoint, double processVar)
{
    // setPoint = r(i), processVar = y(i)

    double P, I, D;
    double dCoeff;

    double e = currentError();

    P = m_k * (m_b * setPoint - processVar);

    // Wyłącz część całkującą jeśli blisko 0.
    if(m_ti < 1e-8)
        m_ti = std::numeric_limits<double>::max();

    I = m_tiSum += m_k * e / m_ti;


    // Wyłącz cz. różniczkującą jeśli blisko 0.
    if(m_td < 1e-8)
        dCoeff = 0;
    else
        dCoeff = m_td / (m_td + m_N);

    // Różniczkowanie (wprzód) z filtracją.
    D = dCoeff * m_Dm1 + m_k*m_N*dCoeff * (e - m_em1);

    // Różniczka idealna
//    if(m_td < 1e-8)
//        D = 0;
//    else
//    D = m_k * m_td * (e - m_em1);

    m_Dm1 = D;
    m_em1 = e;

    // Limit output & anti-windup implementation
    double output = P + I + D;

    double outputLimited = Utils::limitValue(output, m_lowerBound, m_upperBound);

    double limitError = outputLimited - output;

    m_tiSum += limitError / m_ti;

    return outputLimited;

}
// Wersja przyrostowa (nie działa)
//    double bError = setPoint * m_b - processVar;

//    double deltaP, deltaI, deltaD;
//    double P; // P(i).
//    double u; // u(i).

//    // Algorytm przyrostowy.

//    // Część proporcjonalna.
//    P = m_b * setPoint - processVar;
//    deltaP = m_k * P - m_Pm1;
//    m_Pm1 = P;            // Można już nadpisać historię próbki P(i-1).

//    // Część całkująca (najprościej)
//    deltaI = m_k * m_em1 / m_ti;
//    m_em1 = currentError();         // Nadpisujemy historię e(i-1) obecnym uchybem.

//    // Część różniczkująca:
//    deltaD = m_diffB/(1-m_diffA) * (processVar - 2*m_ym1 + m_ym2);
//    m_ym2 = m_ym1;
//    m_ym1 = processVar;

//    // Wyjście regulatora:
//    u = deltaP + deltaI + deltaD + m_um1;
//    m_um1 = u;
//    return u;
//}

//
// Sprawy konfiguracyjne
//

bool PIDController::setParameter(const std::string& param, const std::string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    double tmpVal;
    istr >> tmpVal;

    if(istr.bad())
        throw iconf_error::syntaxError(*this, tvalue);

    if(tparam == CONF_ALL)
        safeSetParameters(tvalue);
    else if(tparam == CONF_GAIN)
        m_k = tmpVal;
    else if(tparam == CONF_B)
        m_b = tmpVal;
    else if(tparam == CONF_TI)
        m_ti = tmpVal;
    else if(tparam == CONF_TD)
        m_td = tmpVal;
    else if(tparam == CONF_N)
        m_N = tmpVal;
    else if(tparam == CONF_UBOUND)
        m_upperBound = tmpVal;
    else if(tparam == CONF_LBOUND)
        m_lowerBound = tmpVal;
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    return true;
}

void PIDController::safeSetParameters(const std::string& values)
{
    // Zapis dający silną gwarancję bezpieczeństwa.
    // Wszystkie parametry są zapisane w formie wektora
    // (jak wielomiany). Podajemy N i Td, a i b muszą być przeliczone.
    // Kolejność jest następująca!
//    double m_k;
//    double m_b;
//    double m_ti;
//    double m_td;
//    double m_N;
//    bool m_forwardDiff;
//    double m_upperBound;
//    double m_lowerBound;

    bool succeed;

    vector<double> vect = Utils::convertStrToVector(values, succeed);

    if(!succeed)
        throw iconf_error::syntaxError(*this, values);

    if(vect.size() != 7)
        throw iconf_error(*this, values, "PID controller has exactly 7 parameters to set.");

    // Poniższe instrukcje muszą się udać:
    m_k = vect[0];
    m_b = vect[1];
    m_ti = vect[2];
    m_td = vect[3];
    m_N = vect[4];
    m_upperBound = vect[5];
    m_lowerBound = vect[6];
}

std::string PIDController::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    ostringstream ostr(outStr);

    if(tparam == CONF_GAIN)
    {
        ostr << m_k;
    }
    else if(tparam == CONF_B)
    {
        ostr << m_b;
    }
    else if(tparam == CONF_TI)
    {
        ostr << m_ti;
    }
    else if(tparam == CONF_TD)
    {
        ostr << m_td;
    }
    else if(tparam == CONF_N)
    {
        ostr << m_N;
    }
    else if(tparam == CONF_UBOUND)
    {
        ostr << m_upperBound;
    }
    else if(tparam == CONF_LBOUND)
    {
        ostr << m_lowerBound;
    }
    else
        return ControllerBase::getParameter(tparam);

    return ostr.str();
}

SimFile::ConfigWriter& PIDController::dumpSpecificPar(SimFile::ConfigWriter& o) const
{
    o.pushParam(CONF_GAIN, m_k);
    o.pushLineComment("Wzmocnienie regulatora.");
    o.pushParam(CONF_B, m_b);
    o.pushLineComment("Wzmocnienie wejścia zadawanego (1.0 - uchyb liczony tradycyjnie).");
    o.pushParam(CONF_TI, m_ti);
    o.pushLineComment("Czas zdwojenia (liczony w próbkach!)");
    o.pushParam(CONF_TD, m_td);
    o.pushLineComment("Czas wyprzedzenia (także w próbkach!)");
    o.pushParam(CONF_N, m_N);
    o.pushLineComment("Współczynnik inercji części różniczkującej.");
    o.pushParam(CONF_UBOUND, m_upperBound);
    o.pushLineComment("Górny próg nasycenia.");
    o.pushParam(CONF_LBOUND, m_lowerBound);
    o.pushLineComment("Dolny próg nasycenia.");
    return o;
}

}
}

